#!/usr/bin/env python3
import rospy
import numpy as np
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
from std_msgs.msg import Header
import numpy as np
import time
import open3d as o3d

# import os
# print(os.path.abspath(os.getcwd()))
width = 100
height = 100




def pointcloud_rasterization(pc_array,voxel_size=1):
    min_coord = np.min(pc_array,axis=0)
    pc_array -= min_coord
    max_coord = np.max(pc_array,axis=0)
    grid_shape = np.ceil(max_coord/voxel_size).astype(np.int32)
    grid_shape[grid_shape == 0] = 1
    grid = np.zeros(grid_shape,dtype=np.int32)

    indices = np.floor(pc_array/voxel_size).astype(np.int32)
    # print(indices)
    for id,i in enumerate(indices):
        grid[indices[id,0],indices[id,1]] +=  1

    return indices
def pub_ore_map(pc_array,pub_pc):
    fields = [
        PointField("x", 0, PointField.FLOAT32, 1),
        PointField("y", 4, PointField.FLOAT32, 1),
        PointField("z", 8, PointField.FLOAT32, 1),
        # PointField("intensity", 12, PointField.FLOAT32, 1),
    ]

    header = Header()
    header.frame_id = "world"
    header.stamp = rospy.Time.now()

    points = pc_array
    pc2 = point_cloud2.create_cloud(header, fields, points)
    pub_pc.publish(pc2)
    print("publist pc")


if __name__ == "__main__":
    rospy.init_node("pc2_publisher")
    pcd_path = rospy.get_param('~pcd_path')
    VOXEL_SIZE = rospy.get_param('~voxel_size')

    pcd = o3d.io.read_point_cloud(pcd_path)
    pcd_array = np.asarray(pcd.points)
    pub = rospy.Publisher("/RamdomOre/pointcloud", PointCloud2, queue_size=100)
    rate = rospy.Rate(1)
    voxel_array = pointcloud_rasterization(pcd_array,voxel_size=VOXEL_SIZE)
    print(voxel_array)


    while not rospy.is_shutdown():
        pub_ore_map(voxel_array,pub)




        rate.sleep()
